#include "fir.h"
#include "math.h"
#include <stdlib.h>
#include "fft.h"
#include "flash.h"
UNDataBuffType fir_cal_databuff;
#define MATH_PI 3.1415926535
U16 x_pointer;
void fir_reset_pointer(void){
	x_pointer = 0;
}
void fir_get_zp(float freq, STRZPNodeType *zps, U16 zpssize, STRZPNodeType *calz, U16 *RN) {
    float fN, fs;
    U32 N;
    U16 i,j,k;
    float dfs, nf;
    float hwfi, hwti, hwf, hwt, haf, hat, pk, pa, hwn, han;
    fs = freq * FIR_SAMPLESIZE_PERPERIOD;
    fN = fs / (freq / 4);
    dfs = freq /4;
    N = (int)fN;
    for(j = 0; j < N; j++) {
        nf = dfs * j;
        hwfi = 0;
        hwti = 100;
        hwf = 1;
        hwt = 1;
        haf = 0;
        hat = 0;
				for(k = 0; k < (zpssize - 1); k++){
					if((nf >= zps[k].freq) && (nf < zps[k+1].freq)){
						
						hwfi = zps[k].freq;
						hwf = zps[k].amp;
						haf = zps[k].angle;

						hwti = zps[k + 1].freq;
						hwt = zps[k + 1].amp;
						hat = zps[k + 1].angle;

						break;
					}
				}
        pk = ((hwt - hwf) / (hwti - hwfi));
        hwn = pk * nf + hwf - pk * hwfi;


        if(hat < haf){
            hat = hat + 2 * MATH_PI;
				}

        pa = ((hat - haf) / (hwti - hwfi));
        han = pa * nf+ haf - pa * hwfi;
				calz[j].amp = nf;
				calz[j].amp = hwn;
				calz[j].angle = han;
    }
		*RN = N;
}

void fir_get_zp_ex(float freq, U16 attr[][2], U16 attrsize, float startfreq, float freqgap, STRZPNodeType *calz, U16 *RN) {
    float fN, fs;
    U32 N;
    U16 i,j,k;
    float dfs, nf;
    float hwfi, hwti, hwf, hwt, haf, hat, pk, pa, hwn, han;
		float lf, uf;
		U32 flashaddr;
		U32 flashcontent[2];
		short lattr[2][2];
    fs = freq * FIR_SAMPLESIZE_PERPERIOD;
    fN = fs / (freq / 4);
    dfs = freq /4;
    N = (int)fN;
    for(j = 0; j < N; j++) {
        nf = dfs * j;
        hwfi = 0;
        hwti = 100;
        hwf = 1;
        hwt = 1;
        haf = 0;
        hat = 0;
				
				for(k = 0; k < (attrsize - 1); k++){
					lf = (float)freqgap * (float)k + startfreq;
					uf = lf + freqgap;
					flashaddr = (U32)k * 8;
					flash_read_data(flashaddr, flashcontent, 2);
					lattr[0][0] = (short)((flashcontent[1] >> 16) & 0xffff);
					lattr[0][1] = (short)(flashcontent[1] & 0xffff);
					
					flash_read_data(flashaddr + 8, flashcontent, 2);
					lattr[1][0] = (short)((flashcontent[1] >> 16) & 0xffff);
					lattr[1][1] = (short)(flashcontent[1] & 0xffff);
					if((nf >= lf) && (nf < uf)){
						
						hwfi = lf;
						hwf = (float)lattr[0][0] / 10000;
						haf = (float)lattr[0][1]/10000;

						hwti = uf;
						hwt = (float)lattr[1][0] / 10000;
						hat = (float)lattr[1][1] / 10000;

						break;
					}
				}
        pk = ((hwt - hwf) / (hwti - hwfi));
        hwn = pk * nf + hwf - pk * hwfi;


        if(hat < haf){
            hat = hat + 2 * MATH_PI;
				}

        pa = ((hat - haf) / (hwti - hwfi));
        han = pa * nf+ haf - pa * hwfi;
				calz[j].freq = nf;
				calz[j].amp = hwn;
				calz[j].angle = han;
    }
		*RN = N;
}

void fir_get_coef(STRZPNodeType *zps, U16 N, float df, U8 linera, float *coef){
	U16 M, k;
	float phase;
	
	M = N + 1;
	for(k = 0; k <= N/2; k++){
		phase = -zps[k].angle;
		fir_cal_databuff.f[k][0] = zps[k].amp * cos(phase);
		fir_cal_databuff.f[k][1] = zps[k].amp * sin(phase);
	}
	for(k = 1; k <= (N/2 - 1); k++){
		fir_cal_databuff.f[N - k][0] = fir_cal_databuff.f[k][0];
		fir_cal_databuff.f[N - k][1] = -fir_cal_databuff.f[k][1];
	}
	fir_cal_databuff.f[0][0] = 0;
	fir_cal_databuff.f[0][1] = 0;
	fnFFT_IFFT(fir_cal_databuff.f, coef);
}


void fir_filter_data(float *x, U16 n, float *firecoef, U16 firn, float *y){
	U16 xp;
	U16 i, j;
	float rt = 0;
	for(i = 0; i < n; i++){
        if(x_pointer >= firn){
          x_pointer = 0;
				}
        fir_cal_databuff.vars[x_pointer] = x[i];
        x_pointer = x_pointer + 1;
        rt = 0;
        xp = x_pointer;
				for(j = 0; j < firn; j++){
          if(xp >= firn){
             xp = 0;
          }
          rt = rt + firecoef[j] * fir_cal_databuff.vars[xp];
          xp = xp + 1;
        }
        y[i] = rt;
		}
}
void fir_get_adcdata(U16 *rsample, float rsamplerate, U16 rsamplesize, float freq, float *esample, float esamplerate, U16 esamplesize){
	 U16 i;
	 float t, dt;
	 float p;
	 float x1, r;
	 U16 i1, i2;
	 float y1, y2, kp;
	 if(abs(rsamplerate - esamplerate) < 1){
		if(esamplesize < rsamplesize){
			for(i = 0; i < esamplesize; i++){
				esample[i] = rsample[i];
			}
			return;
		}
	 }
	 //dt = 1 / esamplerate;
	 //t = 0;
	 //p = 1 / freq;
	 for(i = 0; i < esamplesize; i++){
		 r = 0;
		 t = (float)i * (float)rsamplerate;
		 x1 = t / (float)esamplerate;
		 i1 = (int)x1;
		 i2 = i1 + 1;
		 if(i2 < rsamplesize){
			 y1 = rsample[i1];
			 y2 = rsample[i2];
			 kp = ((float)y2 - (float)y1) / (float)(i2 - i1);
			 
			 r = (float)kp * (float)x1 + (float)y1 - (float)kp * (float)x1;
		 }
		 r = (r + 0.5);
		 esample[i] = r;
	 }
}